#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "math.h"   
#include<chrono>
using namespace std;
using namespace cv;
Vec3b interpolate(const Point2d &p , const Mat &old) //在旧图中进行差值，xy为反旋转后的原始坐标
{

    int a = int(p.x);
    int b = int(p.y);
    int c = a + 1;
    int d = b + 1;
    if (a < 0 ||c > old.rows || b< 0 ||d > old.cols)
    {
        Vec3b result = {0, 0, 0};
        return result;
    }
    Vec3b pix00 = *old.ptr<Vec3b>(a, b);
    Vec3b pix10 = *old.ptr<Vec3b>(a, d);
    Vec3b pix11 = *old.ptr<Vec3b>(c, d);
    Vec3b pix01 = *old.ptr<Vec3b>(c, b);
    if(pix00[0]!=0&&pix00[0]!=255)
    {
        int k=0;
    }
    Vec3b result;
    for (int i=0;i<3;i++)
        result[i] = (pix10[i]- pix00[i]) * (p.x - a) + (pix01[i] - pix00[i]) * (p.y - b) + (pix11[i] + pix00[i] - pix10[i] - pix01[i]) * (p.x - a) * (p.y - b) + pix00[i];
    return result;
}
Point2d invRotateXY(const Point2i &a ,double theta,const double t)
{
    Point2d result;
    result.x=cos(theta)*a.y-sin(theta)*(a.x-t);
    result.y=cos(theta)*(a.x-t)+sin(theta)*a.y;
    return result;
}
Mat rotateTheta(const Mat &m, double theta)
{
    if (theta < 0 || theta > 90)
    {
        cerr << "超出范围" << endl;
        exit(1);
    }
    theta=theta*2*M_PI/360;
    // cout <<sin(theta)<<endl;
    
    double length=int(sqrt(m.size[0]*m.size[0]+m.size[1]*m.size[1]))+1;
    Mat newImg(length,length,16);
    for (int i=0;i<length;i++)
    {
        for(int j =0;j<length;j++)
        {
            Point2i a(j,i);
            double t =m.rows*sin(theta);
            Point2d intP=invRotateXY(a,theta,t);
            Vec3b newPix= interpolate(intP,m);
            for (int p=0;p<3;p++)
                (*newImg.ptr<Vec3b>(i,j))[p]=newPix[p];  // *
        }
    }
    return newImg;
}
int main(int, char **)
{
    auto start = chrono::system_clock::now();
    Mat myimg = imread("../1.png");
    Mat newimg;
    int length = sqrt(myimg.cols * myimg.cols + myimg.rows * myimg.rows) + 10;
    newimg=rotateTheta(myimg,60);
auto end   = chrono:: system_clock::now();
auto duration = chrono::duration_cast<chrono::microseconds>(end - start);
cout <<  "花费了" 
     << double(duration.count()) * chrono::microseconds::period::num / chrono::microseconds::period::den 
     << "秒" << endl;
    imshow("1",myimg);
    waitKey(0);
    imshow("1",newimg);
    try{
      imwrite("result.png",newimg);
    }
    catch(const std::exception& e)
    {
        cout <<e.what()<<endl;
    }
    waitKey(0);
    return 0;
}
